package hdgem.bezier;

import hdgeom.primitives.AbstractShape;
import hdgeom.primitives.PolyLine;

import java.util.ArrayList;
import java.util.Collections;

import processing.core.PGraphics;
import processing.core.PMatrix3D;
import processing.core.PVector;

public class Bezier extends AbstractShape {

	public ArrayList<PVector> controlPoints = new ArrayList<PVector>();

	public Bezier() {

	}

	public Bezier(PVector p1, PVector p2) {
		controlPoints.add(p1);
		controlPoints.add(p2);
	}

	public Bezier(PVector p1, PVector p2, PVector p3) {
		controlPoints.add(p1);
		controlPoints.add(p2);
		controlPoints.add(p3);
	}

	public Bezier(PVector p1, PVector p2, PVector p3, PVector p4) {
		controlPoints.add(p1);
		controlPoints.add(p2);
		controlPoints.add(p3);
		controlPoints.add(p4);
	}

	public void addControlPoint(PVector p) {
		controlPoints.add(p);
	}

	public Bezier getCopy() {
		Bezier bezier = new Bezier();
		for (int i = 0; i < controlPoints.size(); i++) {
			PVector p = controlPoints.get(i);
			bezier.addControlPoint(p.x, p.y, p.z);
		}
		return bezier;
	}

	public void addControlPoint(float x, float y, float z) {
		addControlPoint(new PVector(x, y, z));
	}

	public void inverse() {
		Collections.reverse(controlPoints);
	}

	public PVector getPoint(float t) {
		ArrayList<PVector> cPts = new ArrayList<PVector>(controlPoints);
		while (cPts.size() > 1) {
			ArrayList<PVector> newPts = new ArrayList<PVector>();
			for (int i = 0; i < cPts.size() - 1; i++) {
				PVector p1 = cPts.get(i);
				PVector p2 = cPts.get(i + 1);
				PVector v = PVector.sub(p2, p1);
				v.mult(t);
				v.add(p1);
				newPts.add(v);
			}
			cPts = newPts;
		}
		return cPts.get(0);
	}

	public PVector getTangent(float t) {
		ArrayList<PVector> cPts = new ArrayList<PVector>(controlPoints);
		while (cPts.size() > 2) {
			ArrayList<PVector> newPts = new ArrayList<PVector>();
			for (int i = 0; i < cPts.size() - 1; i++) {
				PVector p1 = cPts.get(i);
				PVector p2 = cPts.get(i + 1);
				PVector v = PVector.sub(p2, p1);
				v.mult(t);
				v.add(p1);
				newPts.add(v);
			}
			cPts = newPts;
		}
		PVector p1 = cPts.get(0);
		PVector p2 = cPts.get(1);
		PVector v = PVector.sub(p2, p1);
		v.normalize();
		return v;
	}

	public PVector getNormal(float t) {
		PVector p1 = getPoint(t);
		PVector p0 = getPoint(t * 0.9f);
		PVector p2 = getPoint(1 - ((1 - t) * 0.9f));
		PVector p10 = PVector.sub(p0, p1);
		p10.normalize();
		PVector p12 = PVector.sub(p2, p1);
		p12.normalize();
		p10.add(p12);
		p10.normalize();
		return p10;
	}

	public PMatrix3D getPMatrixUp(float t, PVector up) {
		// what was the z axis should become the tangent
		// what about one other axis?
		PVector pt = getPoint(t);
		PVector tangent = getTangent(t);
		return getPMatrixPtAxis(pt, tangent, up);
	}

	public static float rZ = 0;

	public static PMatrix3D getPMatrixPtAxis(PVector pt, PVector tangent,
			PVector up) {
		PMatrix3D rot = new PMatrix3D();
		rot.translate(pt.x, pt.y, pt.z);
		PVector s = new PVector(0, 0, 1); // is already normalized (meaning is
											// has length 1)
		tangent.normalize();
		PVector r = s.cross(tangent);
		float a = (float) Math.acos(s.dot(tangent));
		rot.rotate(a, r.x, r.y, r.z);
		PVector v = new PVector(1, 0, 0);
		v = rot.mult(v, v);
		v.sub(pt);
		PVector n1 = v.cross(tangent);
		PVector n2 = up.cross(tangent);
		float angle = signedAngle(n1, n2, tangent);
		rot.rotateZ(angle);
		return rot;
	}

	public static PMatrix3D getPMatrixPtAxis(PVector pt, PVector tangent) {
		PMatrix3D rot = new PMatrix3D();
		rot.translate(pt.x, pt.y, pt.z);
		PVector s = new PVector(0, 0, 1); // is already normalized (meaning is
											// has length 1)
		tangent.normalize();
		PVector r = s.cross(tangent);
		float a = (float) Math.acos(s.dot(tangent));
		rot.rotate(a, r.x, r.y, r.z);
		return rot;
	}

	public static float angleTan(PVector a, PVector b) {
		float dot = a.dot(b);
		if (dot == 0) {
			return 0;
		}
		return (float) Math.atan2(a.cross(b).mag(), a.dot(b));
	}

	public static float angleCos(PVector Va, PVector Vb) {
		return (float) Math.acos(PVector.dot(Va, Vb) / (Va.cross(Vb).mag()));
	}

	public static float signedAngle(PVector Va, PVector Vb, PVector n) {
		Va.normalize();
		Vb.normalize();
		float dot = Va.dot(Vb);
		PVector crossV = Va.cross(Vb);
		if (crossV.mag() <= 0.00001f) {
			if (dot < 0) {
				return (float) Math.PI;
			}
			return 0;
		}
		float theta = (float) Math.atan2(crossV.mag(), dot)
				* Math.signum(PVector.dot(crossV, n));
		return (float) theta;
	}

	public static PMatrix3D getPMatrix(PVector v1, PVector v2) {
		PVector v1v2 = PVector.sub(v2, v1);
		v1v2.normalize();
		return getPMatrixPtAxis(v1, v1v2);
	}

	public PolyLine getPolyLine(int i) {
		PolyLine poly = new PolyLine();
		for (int j = 0; j < i + 1; j++) {
			PVector cPt = getPoint(j * 1f / i);
			poly.vertices.add(cPt);
		}
		return poly;
	}

	public void display3D(PGraphics g, int i) {
		PVector oldP = controlPoints.get(0);
		for (int j = 0; j < i + 1; j++) {
			PVector cPt = getPoint(j * 1f / i);
			g.line(oldP.x, oldP.y, oldP.z, cPt.x, cPt.y, cPt.z);
			oldP = cPt;
		}
	}

	public void display2D(PGraphics g, int i) {
		PVector oldP = controlPoints.get(0);
		for (int j = 0; j < i + 1; j++) {
			PVector cPt = getPoint(j * 1f / i);
			g.line(oldP.x, oldP.y, cPt.x, cPt.y);
			oldP = cPt;
		}
	}
	
	public void displayCap(PGraphics g, ArrayList<PVector> profile, float p,
			boolean dir, PVector up) {

		PMatrix3D newM = getPMatrixUp(p, up);
		g.pushMatrix();
		g.applyMatrix(newM);
		g.beginShape(PGraphics.TRIANGLE_FAN);
		g.vertex(0, 0, 0);
		if (dir) {
			for (int ii = profile.size(); ii >= 0; ii--) {
				PVector v1 = profile.get(ii % profile.size());
				g.vertex(v1.x, v1.y, v1.z);
			}
		} else {
			for (int ii = 0; ii <= profile.size(); ii++) {
				PVector v1 = profile.get(ii % profile.size());
				g.vertex(v1.x, v1.y, v1.z);
			}
		}
		g.endShape();
		g.popMatrix();
	}
	public void displayExtrude(PGraphics g, ArrayList<PVector> profile,
			int res, boolean caps) {
		PMatrix3D oldM = null;
		PVector result = new PVector();
		for (int i = 0; i < res + 1; i++) {
			float t = i * 1f / res;
			PVector p=getPoint(t);
			PVector tan=getTangent(t);
			PMatrix3D newM = getPMatrixPtAxis(p, tan);
			if (oldM != null) {
				g.beginShape(PGraphics.QUAD_STRIP);
				for (int ii = 0; ii <= profile.size(); ii++) {
					PVector v1 = profile.get(ii % profile.size());
					oldM.mult(v1, result);
					g.vertex(result.x, result.y, result.z);
					newM.mult(v1, result);
					g.vertex(result.x, result.y, result.z);
				}
				g.endShape();
			}
			oldM = newM;
		}

		if (caps) {
			//displayCap(g, profile, 0, true);
			//displayCap(g, profile, 1, false);
		}
	}
	public void displayExtrudeUp(PGraphics g, ArrayList<PVector> profile,
			int res, boolean caps, PVector up) {
		PMatrix3D oldM = null;
		PVector result = new PVector();
		for (int i = 0; i < res + 1; i++) {
			float t = i * 1f / res;
			PMatrix3D newM = getPMatrixUp(t, up);
			if (oldM != null) {
				g.beginShape(PGraphics.QUAD_STRIP);
				for (int ii = 0; ii <= profile.size(); ii++) {
					PVector v1 = profile.get(ii % profile.size());
					oldM.mult(v1, result);
					g.vertex(result.x, result.y, result.z);
					newM.mult(v1, result);
					g.vertex(result.x, result.y, result.z);
				}
				g.endShape();
			}
			oldM = newM;
		}

		if (caps) {
			displayCap(g, profile, 0, true, up);
			displayCap(g, profile, 1, false, up);
		}
	}

	public void displayExtrudeUp(PGraphics g, ProfileFabric pf, int res,
			boolean caps, PVector up) {
		PMatrix3D oldM = null;
		PVector result = new PVector();
		for (int i = 0; i < res + 1; i++) {
			float t = i * 1f / res;
			PMatrix3D newM = getPMatrixUp(t, up);
			ArrayList<PVector> profile = pf.getProfile(t);
			if (oldM != null) {
				g.beginShape(PGraphics.QUAD_STRIP);
				for (int ii = 0; ii <= profile.size(); ii++) {
					PVector v1 = profile.get(ii % profile.size());
					oldM.mult(v1, result);
					g.vertex(result.x, result.y, result.z);
					newM.mult(v1, result);
					g.vertex(result.x, result.y, result.z);
				}
				g.endShape();
			}
			oldM = newM;
		}

		if (caps) {

			displayCap(g, pf.getProfile(0), 0, true, up);
			displayCap(g, pf.getProfile(1), 1, false, up);
		}
	}

	public static float dist(float x1, float y1, float x2, float y2) {
		float dX = x2 - x1;
		float dY = y2 - y1;
		return (float) Math.sqrt(dX * dX + dY * dY);
	}

	public ArrayList<Bezier> splitBezier(float t) {
		return null;
	}

	public void splitBezier(float t, Bezier b1, Bezier b2) {
		ArrayList<PVector> cPts = new ArrayList<PVector>(controlPoints);
		while (cPts.size() > 1) {
			ArrayList<PVector> newPts = new ArrayList<PVector>();
			b1.addControlPoint(cPts.get(0).get());
			b2.addControlPoint(cPts.get(cPts.size() - 1).get());
			for (int i = 0; i < cPts.size() - 1; i++) {
				PVector p1 = cPts.get(i);
				PVector p2 = cPts.get(i + 1);
				PVector v = PVector.sub(p2, p1);
				v.mult(t);
				v.add(p1);
				newPts.add(v);
			}
			cPts = newPts;
		}
		PVector p = cPts.get(0);
		b1.addControlPoint(new PVector(p.x, p.y, p.z));
		b2.addControlPoint(new PVector(p.x, p.y, p.z));
		Collections.reverse(b2.controlPoints);
	}

	public static PVector rotateAroundLine(PVector p, PVector o, PVector vec,
			float angle) {
		// http://inside.mines.edu/~gmurray/ArbitraryAxisRotation/
		// If we multiply this times ⟨x,y,z⟩ we can obtain a function of of
		// ten variables that yields the result of
		// rotating the point (x,y,z) about the line through (a,b,c) with
		// direction vector ⟨u,v,w⟩ (where u2 + v2 + w2 = 1) by the angle
		// θ.
		float x = p.x;
		float y = p.y;
		float z = p.z;
		float a = o.x;
		float b = o.y;
		float c = o.z;
		float u = vec.x;
		float v = vec.y;
		float w = vec.z;
		float cos = (float) Math.cos(angle);
		float sin = (float) Math.sin(angle);
		float newX = (a * (v * v + w * w) - u
				* (b * v + c * w - u * x - v * y - w * z))
				* (1 - cos) + x * cos + (-c * v + b * w - w * y + v * z) * sin;
		float newY = (b * (u * u + w * w) - v
				* (a * u - c * w - u * x - v * y - w * z))
				* (1 - cos) + y * cos + (c * u - a * w + w * x - u * z) * sin;
		float newZ = (c * (u * u + v * v) - w
				* (a * u + b * v - u * x - v * y - w * z))
				* (1 - cos) + z * cos + (-b * u + a * v - v * x + u * y) * sin;
		return new PVector(newX, newY, newZ);
	}

	@Override
	public int getNumVertices() {
		// TODO Auto-generated method stub
		return controlPoints.size();
	}

	@Override
	public void getPosition(int i, float[] coords) {
		// TODO Auto-generated method stub
		PVector p = controlPoints.get(i);
		coords[0] = p.x;
		coords[1] = p.y;
		coords[2] = p.z;
	}

	@Override
	public void setPosition(int i, float x, float y, float z) {
		// TODO Auto-generated method stub
		PVector p = controlPoints.get(i);
		p.set(x, y, z);
	}

	@Override
	public void display2D(PGraphics g) {
		// TODO Auto-generated method stub

	}

	@Override
	public void display3D(PGraphics g) {
		// TODO Auto-generated method stub

	}

}
